#include <cmath>
#include "math_utils.h"

DqVoltage_t voltage{};

const int SIN_MAP[200] = {0, 79, 158, 237, 316, 395, 473, 552, 631, 710,
                          789, 867, 946, 1024, 1103, 1181, 1260, 1338, 1416, 1494,
                          1572, 1650, 1728, 1806, 1883, 1961, 2038, 2115, 2192, 2269,
                          2346, 2423, 2499, 2575, 2652, 2728, 2804, 2879, 2955, 3030,
                          3105, 3180, 3255, 3329, 3404, 3478, 3552, 3625, 3699, 3772,
                          3845, 3918, 3990, 4063, 4135, 4206, 4278, 4349, 4420, 4491,
                          4561, 4631, 4701, 4770, 4840, 4909, 4977, 5046, 5113, 5181,
                          5249, 5316, 5382, 5449, 5515, 5580, 5646, 5711, 5775, 5839,
                          5903, 5967, 6030, 6093, 6155, 6217, 6279, 6340, 6401, 6461,
                          6521, 6581, 6640, 6699, 6758, 6815, 6873, 6930, 6987, 7043,
                          7099, 7154, 7209, 7264, 7318, 7371, 7424, 7477, 7529, 7581,
                          7632, 7683, 7733, 7783, 7832, 7881, 7930, 7977, 8025, 8072,
                          8118, 8164, 8209, 8254, 8298, 8342, 8385, 8428, 8470, 8512,
                          8553, 8594, 8634, 8673, 8712, 8751, 8789, 8826, 8863, 8899,
                          8935, 8970, 9005, 9039, 9072, 9105, 9138, 9169, 9201, 9231,
                          9261, 9291, 9320, 9348, 9376, 9403, 9429, 9455, 9481, 9506,
                          9530, 9554, 9577, 9599, 9621, 9642, 9663, 9683, 9702, 9721,
                          9739, 9757, 9774, 9790, 9806, 9821, 9836, 9850, 9863, 9876,
                          9888, 9899, 9910, 9920, 9930, 9939, 9947, 9955, 9962, 9969,
                          9975, 9980, 9985, 9989, 9992, 9995, 9997, 9999, 10000, 10000};

// 求a对应的sin值，对应0~1，返回浮点型
float SinApprox(float a)
{
    if (a < _PI_2)
    {
        return 0.0001f * (float)SIN_MAP[ROUND(126.6873f * a)];
    }
    else if (a < _PI)
    {
        return 0.0001f * (float)SIN_MAP[398 - ROUND(126.6873f * a)];
    }
    else if (a < _3PI_2)
    {
        return -0.0001f * (float)SIN_MAP[-398 + ROUND(126.6873f * a)];
    }
    else
    {
        return -0.0001f * (float)SIN_MAP[796 - ROUND(126.6873f * a)];
    }
}

// 求a对应的cos值，对应0~1，返回浮点型
float CosApprox(float a)
{
    float aSin = a + _PI_2;
    aSin = aSin > _2PI ? aSin - _2PI : aSin;

    return SinApprox(aSin);
}

// 取余，相当于限制angle在2PI内
float Normalize(float angle)
{
    float a = std::fmod(angle, _2PI); // 返回angle/_2PI的余数，浮点型
    return a >= 0 ? a : (a + _2PI);
}

// Square root approximation function using
// https://reprap.org/forum/read.php?147,219210
// https://en.wikipedia.org/wiki/Fast_inverse_square_root
// 快速求开平方数，返回浮点型
float SqrtApprox(float value)
{
    long i;
    float y;

    y = value;
    i = *(long *)&y;
    i = 0x5f375a86 - (i >> 1);
    y = *(float *)&i;

    return value * y;
}
